#ifndef __PATHMAP__
#define __PATHMAP__

#include "SDLittleDog.h"
#include "Node.h"
#include <stdlib.h>
#include <iostream>
#include <vector>

using namespace std;

struct PathMapNode{
    float f; //h + g
    float g; //cost
    float h; //heuristic
    int y; //y coord where the node is at in the grid
    int x;
    PathMapNode* parent;
};

struct Point{
    float x;
    float y;
};

class PathMap{

    public:

       PathMap(SDSimulator* sim, float gS, float mX, float mxX, float mY, float mxY); 

       ~PathMap();

       float getHMapAt(float x, float y);

       float getGMapAt(float x, float y);
	
	   vector<Point*> findPath();

       Point* getBestStep(Node* current, float xToMove, float yToMove, int numCheckSize); 

       void printGradMapToFile(const char*filename);

       void printHeightMapToFile(const char*filename);
       
       void printPathToFile(const char*filename,vector<Point*> path);

       void setTerrainWeight(float newWeight);

       void addObstacle(Point* point);

    private:

       PathMapNode* CreateFringeNode(PathMapNode* current, int i, int j, int moveSize);

       vector<Point*> convertNodeToPath(PathMapNode* goalNode);

       float getNodeCost(PathMapNode* current, int pathWidth);

       SDSimulator* simulator;
       float gridSize; //resolution of the maps
       float stepSizeMetric; //space between two squares of the path search
       float pathWidthMetric; //half-size of a square of the path search
       float terrainWeight; // the weight applied to the terrain difficulty in the cost function.
       float minX, maxX, minY, maxY;//edges of the maps (metric)
       float** hmap;
       float** gmap;
       int numXvalues; // size of maps in x direction
       int numYvalues; // size of maps in y direction

};


#endif
